#include "zmath.h"


#include <QDebug>

const double XMinDifference=0.0000001;
const double YMinDifference=0.0000001;
const double PI=3.1415926535897932384626433832795;
const double MaxSlope=1000000000;
const double MinSlope=0.000000001;
const double MinDistance=0.000000001;

double Slope(QPointF P0,QPointF P1)
{
    double tSlope;
    double tXDiff,tYDiff;
    tXDiff=P1.x()-P0.x();
    tYDiff=P1.y()-P0.y();

    if(abs(tXDiff)<XMinDifference)
    {
        if (P1.x()>P0.x())
        {
            tXDiff=XMinDifference;
        }
        else
        {
            tXDiff=-XMinDifference;
        }
    }
    tSlope= tYDiff/tXDiff;
    if (tSlope>=MaxSlope) tSlope=MaxSlope;
    if (tSlope<=-MaxSlope) tSlope=-MaxSlope;
    if (abs(tSlope)<MinSlope) tSlope=0;
    return tSlope;
}

double VerticalSlope(double nSlope)
{
    if (nSlope>=MaxSlope ||nSlope<=-MaxSlope)
    {
        return 0;
    }
    else if (abs(nSlope)<MinSlope)
    {
        if (nSlope>0)
            return -MaxSlope;
        else
            return MaxSlope;
    }
    else
    {
        return -1/nSlope;
    }
}

/*
 * 计算航向角
 * P0和P1 的射线相对于正下方（x=0,y=nan） 逆时针旋转过的角度
 *
*/

//在此使用设计书中第一种改错方式修改
double HeadingAng(QPointF P0,QPointF P1)
{
    double tXDiff,tYDiff;
    tXDiff=P1.x()-P0.x();
    tYDiff=P1.y()-P0.y();
    if (tXDiff>=0)
    {
        if (tYDiff>YMinDifference)
        {
            return atan(tXDiff/tYDiff)/PI*180;
        }
        else if (tYDiff<-YMinDifference)
        {
            return atan(tXDiff/tYDiff)/PI*180+180;
        }
        else
        {
            if(tXDiff>1)
            {
                return 90;
            }
            else
            {
                /*修改前
                if (tYDiff>0)
                {
                    return atan(tXDiff/YMinDifference)/PI;
                }
                else
                {
                    return atan(tXDiff/-YMinDifference)/PI;
                }
                */
                if (tYDiff>0)
                {
                    return atan(tXDiff/YMinDifference)/PI*180;
                }
                else
                {
                    return atan(tXDiff/-YMinDifference)/PI*180+180;
                }
            }

        }
    }
    else
    {
        if (tYDiff>YMinDifference)
        {
            return atan(tXDiff/tYDiff)/PI*180+360;
        }
        else if (tYDiff<-YMinDifference)
        {
            return atan(tXDiff/tYDiff)/PI*180+180;
        }
        else
        {
            if (tXDiff<-1)
            {
                return 270;
            }
            else
            {
                /*修改前
                if (tYDiff>0)
                {
                    return atan(tXDiff/YMinDifference)/PI+180;
                }
                else
                {
                    return atan(tXDiff/-YMinDifference)/PI+180;
                }
                */
                if (tYDiff>0)
                {
                    return atan(tXDiff/YMinDifference)/PI*180+360;
                }
                else if(tYDiff<0)
                {
                    return atan(tXDiff/-YMinDifference)/PI*180+180;
                }
                else
                {
                    return 270;
                }
            }
        }
    }
}

double DistanceP2P(QPointF P0,QPointF P1)
{
    return sqrt((P1.x()-P0.x())*(P1.x()-P0.x())+(P1.y()-P0.y())*(P1.y()-P0.y()));
}
/*
 * 计算车辆在路径左边还是右边
 *
*/

double AngAtBaseCoordinate(QPointF PBase,QPointF PDir,QPointF PIn)
{
    // turn left prior
    double tAng,tAng1,tAng2;
    tAng1=HeadingAng(PBase,PDir);
    tAng2=HeadingAng(PBase,PIn);
    //qDebug() << "tang1:" << tAng1 <<"tang2:" << tAng2;
    tAng=tAng2-tAng1;
    if (tAng<=-180) tAng=tAng+360;
    if (tAng>180)  tAng=-360+tAng;
    return tAng;
}

//第二种修改方法，设计新的函数判断计算车辆在路径左边还是右边
/*
double AngAtBaseCoordinate(QPointF PBase,QPointF PDir,QPointF PIn)
{
    double tAng = 0.0;
    if(abs(PDir.x()-PBase.x()) > 1e-05 )
    {

        double k = (PDir.y() - PBase.y()) / (PDir.x()-PBase.x());
        double b = PBase.y() - k*PBase.x();
        double dNum = k*PIn.x() + b;
        if(PDir.x()-PBase.x() > 0)
        {
            if(dNum > PIn.y())
            {
                tAng = 10.0;
            }
            else if(dNum < PIn.y())
            {
                tAng = -10.0;
            }
            else
                tAng = 0.0;
        }
        if(PDir.x()-PBase.x() < 0)
        {
            if(dNum > PIn.y())
            {
                tAng = -10.0;
            }
            else if(dNum < PIn.y())
            {
                tAng = 10.0;
            }
            else
                tAng = 0.0;
        }

 //       qDebug() << dNum << PIn.y() << k << PDir.x() - PBase.x();

    }
    else
    {
        if(PDir.y()-PBase.y()<0)
        {
            if(PIn.x()<PBase.x())
            {
                tAng = 10.0;
            }
            else if(PIn.x()>PBase.x())
                tAng = -10.0;
            else
                tAng = 0.0;
        }
        else if(PDir.y()-PBase.y()>0)
        {
            if(PIn.x()<PBase.x())
            {
                tAng = -10.0;
            }
            else if(PIn.x()>PBase.x())
                tAng = 10.0;
            else
                tAng = 0.0;
        }

    }
    return tAng;

}
*/

/*
 * 获取垂足
 *
*/

QPointF FootPoint(QPointF StartP,QPointF EndP,QPointF ThirdP)
{
    QPointF tFootP;
    double tSlope=Slope(StartP,EndP);
    double tVSlope=VerticalSlope(tSlope);
    if (abs(tSlope)>=MaxSlope)
    {
        tFootP.setX(StartP.x());
        tFootP.setY(ThirdP.y());
    }
    else if (abs(tSlope)<=MinSlope)
    {
        tFootP.setX(ThirdP.x());
        tFootP.setY(StartP.y());
    }
    else
    {
        //StartP,EndP line y=tSlope*x+a
        //thirdP,foot Line y=tVSlope*x+b
        double a=StartP.y()-tSlope*StartP.x();
        double b=ThirdP.y()-tVSlope*ThirdP.x();
        tFootP.setX((b-a)/(tSlope-tVSlope));
        tFootP.setY(tVSlope*tFootP.x()+b);
    }
    return tFootP;
}

/*
 * 通过垂足做偏移
 *
*/

QPointF OffsetPoint(QPointF BaseP,QPointF P0, QPointF P1,double offset)
{
    double tSlope=Slope(P0,P1);
    QPointF tOffsetP;
    if (abs(tSlope)>=MaxSlope)
    {
        tOffsetP.setX(BaseP.x());
        if (P1.y()>P0.y())
        {
            tOffsetP.setY(BaseP.y()+offset);
        }
        else
        {
            tOffsetP.setY(BaseP.y()-offset);
        }
    }
    else if (abs(tSlope)<=MinSlope)
    {
        if (P1.x()>P0.x())
        {
            tOffsetP.setX(BaseP.x()+offset);
        }
        else
        {
            tOffsetP.setX(BaseP.x()-offset);
        }
        tOffsetP.setY(BaseP.y());
    }
    else
    {
        if (P1.x()>P0.x())
        {
            tOffsetP.setX(BaseP.x()+offset/sqrt(1+tSlope*tSlope));
            tOffsetP.setY(BaseP.y()+offset/sqrt(1+tSlope*tSlope)*tSlope);
        }
        else
        {
            tOffsetP.setX(BaseP.x()-offset/sqrt(1+tSlope*tSlope));
            tOffsetP.setY(BaseP.y()-offset/sqrt(1+tSlope*tSlope)*tSlope);
        }
    }
    return tOffsetP;
}
/*
 * 确定预瞄点位置P0是起点，P1是终点，P2是预瞄点
 *
*/
int PointPostion(QPointF P0,QPointF P1,QPointF P2)
{
    // P0 P1 P2 return 1
    // P0 P2 P1 return 0
    // P2 P0 P1 return -1

    double tLength01=DistanceP2P(P0,P1);
    double tLength02=DistanceP2P(P0,P2);
    double tLength12=DistanceP2P(P1,P2);
    if (tLength02<MinDistance)
    {
        return 0;
    }
    if (tLength12<MinDistance)
    {
        return 0;
    }
    if (tLength02>tLength01 && tLength02>tLength12)
    {
        return 1;
    }
    if (tLength12>tLength01 && tLength12>tLength02)
    {
        return -1;
    }
    return 0;
}

